#if 1
/*
Compute elastic reactions using

W(C) = lambda tr(C)^2 + mu/2 tr(C^2)
D_C(W) = mu (C : . )  + lambda tr(C) (I  :  . )
=   A :  (.)

with

A = (mu C + lambda I1 Id)


this gives

Sigma = lambda  tr(E) + 2 mu E
E = 1/2 ((I + Grad U )^T (I + Grad U) - I)

This is linear material combined with nonlinear geometry.


For small deformations, you get more numerical precision since
F = I + grad U, where grad U = Oh(Eps), and I = Oh(1).

Working with E, E = Oh(Eps), and you loose less significant digits. 

*/

#include <cstdio>

#include "TetrahedronState.h"
#include "Mechanics.h"



Real PicinbonoElasticEnergy (Real I1, Real I2, Real I3)
{
	// todo : gamma = 0.
	return 0.5* (-lame_mu - lame_lambda * 1.5 * I1
		+ (lame_lambda *0.25 + lame_mu * 0.5)*Sqr (I1)
		- lame_mu * I2);
}


#define PARANOIA 0

/*
Compute STRESS from STRAIN. STRESS and STRAIN may be aliased to each
other.

COST: 15 flops

(setq picinbono_constitutive_equation (+ Matrix3::trace Matrix3::scale Matrix3::add_diag))

*/
static void PicinbonoConstitutiveEquation (Matrix3 *stress, Matrix3 const &strain)
{
	Real tr = strain.Trace();
	if (stress != &strain)
		*stress = strain;

	Matrix3::Scale (*stress, 2*lame_mu, *stress);
	stress->AddDiag (lame_lambda *tr);
}




static void PicinbonoCalcStrain (TetrahedronState const*me,
					   Matrix3 *strain_dest,
					   Matrix3  const&displacement_mat);
/*
Compute the 2nd derivative of the energy function, ie. the hessian of
the energy function.  In the linear case, this is a constant matrix,
in the nonlinear case, K depends on u.  Since we do not store
explicit matrices, we do not return the matrix itself, but the result
of multiplying K, evaluated in DISPLACEMENT, with SOUREC_VEC. The
result is stored in DEST_VEC.

COST

(+ picinbono_calc_strain picinbono_constitutive_equation
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::add_diag 
Matrix3::multiply_mtm
Matrix3::symmetrify
picinbono_constitutive_equation
Matrix3::add
Matrix3::multiply_mtm
Matrix3::multiply_mtm
Matrix3::multiply_mtm   
Matrix3::negate)


456
*/
int PicinbonoForceDerivative (TetrahedronState const *me,
							Matrix3 *dforce,
							Matrix3 *force,
							Matrix3 const& displacement_mat,
							Matrix3 const& source_mat)
{

	Matrix3 E;
	PicinbonoCalcStrain (me, &E, displacement_mat); // 84

	Matrix3 T;
	PicinbonoConstitutiveEquation (&T, E); // 15

	Matrix3 DF, F;

	Matrix3::MultiplyMM (DF, source_mat, me->inverse_location_); // 54
	/*
	DF = (.) * X^{-1}
	*/

	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_); // 54
	F.AddDiag (1.0);

	Matrix3 DE;
	Matrix3::MultiplyMTM (DE, F, DF); // 54
	Matrix3::Symmetrify (DE, DE); // 12

	/*
	D strain = 1/2 D(F^T F - I)
	= symmetrify (F^T DF)
	*/

	Matrix3 DT ;
	PicinbonoConstitutiveEquation (&DT, DE);

	Matrix3::Add (*dforce, DF * T, F * DT);
	*dforce = (*dforce) * me->invloc_transpose_; // 54

	Matrix3::Negate (*dforce , *dforce); // 9

	int flops = 456;
	if (force)
	{
		/*
		(+  (* 2 Matrix3::multiply_mtm) Matrix3::negate)


		*/
		*force =  F  * T * me->invloc_transpose_;
		Matrix3::Negate (*force, *force);
		flops += 108;
	}

#if 1 // PARANOIA
	Matrix3 f2, df2;
	Picinbono2ForceDerivative (me, &df2, &f2, displacement_mat, source_mat);

	Real diff = (df2 - (*dforce)).HilbertSchmidtNorm ();
	Real df2h = df2.HilbertSchmidtNorm ();
	Real fdiff = (*force  - f2).HilbertSchmidtNorm ();
	Real f2h =f2.HilbertSchmidtNorm ();
	if (diff > 1e-6  * df2h)
	{
		fprintf (stderr, "d DF / F = %lf\n", diff /df2h);
		fprintf (stderr, "d F / F = %lf\n", fdiff /f2h);      
	}
#endif

	return flops;
}


/*
The non-linear elastic force from tetrahedron ME, is put into
GRAD_VEC from DISPLACEMENT_VEC.

COST:

(+ picinbono_calc_strain picinbono_constitutive_equation Matrix3::multiply_mtm
Matrix3::add_diag
Matrix3::multiply_mtm
Matrix3::multiply_mtm
Matrix3::negate)

*/
int PicinbonoElasticForce (TetrahedronState const*me,
						 Matrix3  * forces,
						 Matrix3  const &displacement_mat)
{
	if (me->degenerate_b_)
		return 0;

	Matrix3 strain;
	Matrix3 stress (strain);

	PicinbonoCalcStrain (me, &strain, displacement_mat);
	PicinbonoConstitutiveEquation ( &stress, strain);

	Matrix3 F;

	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	F.AddDiag (1.0);

	*forces = F * stress * me->invloc_transpose_;

	// force = - dW / dU
	Matrix3::Negate (*forces, *forces);

	int flops = 264;



#if PARANOIA
	Real tol= 1.0e-5; 
	Matrix3 C;
	Matrix3::Square (C, F);

	Real I1 = C.Trace ();

	Matrix3 A;
	A.Diag (- lame_mu +  lame_lambda* (0.5 *I1 -1.5));

	Matrix3::Saxpy (A, lame_mu, C, A);

	Real dT =(A- stress).HilbertSchmidtNorm ();
	Real T = stress.HilbertSchmidtNorm();
	if (dT > tol * T)
	{
		fprintf (stderr, "|dT|/|T| = %f",  dT /T);
	}

	Matrix3 f2, f3;

	Picinbono2ElasticForce (me, &f2, displacement_mat);
	Matrix3::Subtract (f3, *forces, f2);
	Real fh = forces->HilbertSchmidtNorm ();
	Real f3h =f3.HilbertSchmidtNorm ();
	if (f3h > fh*tol && fh)
	{
		fprintf (stderr, "|df/f| = %f\n", f3h/fh);
	}
#endif

	return flops;
}


/*
COST:
(setq picinbono_calc_strain (+
Matrix3::multiply_mm
Matrix3::square
Matrix3::double_symmetrify
Matrix3::add
Matrix3::scale))

84
*/
void PicinbonoCalcStrain (TetrahedronState const*me,
					   Matrix3 *strain_dest,
					   Matrix3  const&displacement_mat) 
{
	assert ( &displacement_mat != strain_dest);
	if (me->degenerate_b_)
		return;

	Matrix3::MultiplyMM (*strain_dest, displacement_mat, me->inverse_location_);

	Matrix3 quad ;

	/*
	Note: strain^T * strain, not vice versa.
	*/
	Matrix3::Square (quad,  *strain_dest);
	Matrix3::DoubleSymmetrify (*strain_dest, *strain_dest);
	Matrix3::Add (*strain_dest, quad, *strain_dest); // 9
	Matrix3::Scale (*strain_dest, 0.5, *strain_dest); // 9


#if PARANOIA
	Matrix3 C;
	Matrix3 F;

	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	C.AddDiag (-1.0);
	Matrix3 E = 0.5 * C;

	Real diff = (E - *strain_dest).HilbertSchmidtNorm ();
	Real eh =E.HilbertSchmidtNorm();
	if (eh && diff > eh *1e-6)
	{
		fprintf (stderr, "|dE|/|E| = %lf\n", diff /eh);
	}
#endif
}

#if 0

#define F me->precompute_mat_[0]
#define nlstress me->precompute_mat_[2]

/*
COST:

(+ 198  15 9 54 3)  =  279
*/
int PicinbonoPrecomputeForDisplacement(TetrahedronState *me,
									  Matrix3 const& displacement_mat)
{
	picinbonoCalcStrain (me, &nlstress, displacement_mat);
	picinbonoConstitutiveEquation (&nlstress, nlstress);

	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	for (int i=DIM; i--;) 
		F(i,i) +=  1.0;

	return 279;
}

/*
COST

(+ 9 54 54 12 9 54 54 54 9 18) = 327 flops

force vec:

(+
) 
*/
int PicinbonoIncrementalForceDerivative (TetrahedronState const *me,
										Matrix3 *dforce,
										Matrix3 const &source_mat)
{
	Matrix3 dstrain, DF;
	Matrix3::MultiplyMM (DF, source_mat, me->inverse_location_);

	/*
	DF = (.) * X^{-1}
	*/

	Matrix3::MultiplyMTM (dstrain, F, DF);	// 54
	Matrix3::Symmetrify (dstrain, dstrain); //  12

	/*
	D strain = symmetrify (F^T DF )
	*/
	Matrix3 dstress ;
	picinbono_constitutive_equation ( &dstress, dstrain); // 15 

	Matrix3::add (*dforce, DF * nlstress, F * dstress ); // 9 54 54

	*dforce = *dforce * me->invloc_transpose_; // 54

	Matrix3::negate (*dforce , *dforce); // 9


	return 327;  
}
#endif
#endif
